#include "hwtimerreload.h"
#include "ltimer.h"

#define _STRX(x)    #x

#ifdef QT_VERSION
#include <QDebug>
#define _DEBUG(x, arg...) qDebug(x, ##arg)
#else
#include <stdio.h>
#define _DEBUG(x, arg...) printf(x"\n", ##arg)
#endif

rt_uint32_t calcTimerReload1(rt_hwtimer_t *timer, rt_hwtimerval_t *tv)
{
    float overflow;
    float timeout;
    rt_uint32_t counter;
    int i, index = 0;
    float tv_sec;
    float devi_min = 1;
    float devi;

    /* changed to second */
    overflow = timer->maxcnt/(float)timer->freq;
    tv_sec = tv->sec + tv->usec/(float)1000000;

    if (tv_sec < (1/(float)timer->freq))
    {
        /* little timeout */
        i = 0;
        timeout = 1/(float)timer->freq;
    }
    else
    {
        for (i = 1; i > 0; i ++)
        {
            timeout = tv_sec/i;

            if (timeout <= overflow)
            {
                counter = timeout*timer->freq;
                devi = tv_sec - (counter/(float)timer->freq)*i;
                /* Minimum calculation error */
                if (devi > devi_min)
                {
                    i = index;
                    timeout = tv_sec/i;
                    break;
                }
                else if (devi == 0)
                {
                    break;
                }
                else if (devi < devi_min)
                {
                    devi_min = devi;
                    index = i;
                }
            }
        }
    }

    timer->cycles = i;
    timer->reload = i;
    timer->period_sec = timeout;
    counter = timeout*timer->freq;

    return counter;
}

rt_uint32_t calcTimerReload32(rt_hwtimer_t *timer, rt_hwtimerval_t *tv)
{
    rt_uint32_t counter, reload;
    rt_uint32_t timer_cnt, n1;
    int i, index = 0, n0;
    float tv_sec;
    rt_uint32_t dev, dev_min;

    /* changed to second */
    tv_sec = tv->sec + tv->usec/(float)1000000.0;
    timer_cnt = tv_sec * timer->freq;
    if (timer_cnt == 0) {
        timer_cnt = 1;
    }
    if (timer_cnt < timer->maxcnt) {
        timer->cycles = timer->reload = 1;
        timer->period_sec = tv_sec;
        counter = timer_cnt;
        return counter;
    }
    if (timer_cnt % timer->maxcnt == 0) {
        timer->cycles = timer->reload = timer_cnt / timer->maxcnt;
        timer->period_sec = tv_sec;
        counter = timer_cnt;
        return counter;
    }
    n0 = timer_cnt / timer->maxcnt + 1;
    n1 = timer_cnt / 2;
    dev_min = n0;
    for (i = n0; i < n1; i++) {
        reload = (rt_uint32_t)(timer_cnt / i);
        dev = timer_cnt - reload * i;
        if (dev <= 10) {
            // end
            index = i;
            break;
#ifdef HIGH_SPEED
        } else if (dev > dev_min) {
            break;
#endif
        } else if (dev < dev_min) {
            dev_min = dev;
            index = i;
        }
    }
    timer->cycles = timer->reload = index;
    timer->period_sec = index / timer->freq;
    counter = timer_cnt / index;
    return counter;
}

rt_uint32_t calcTimerReload64(rt_hwtimer_t *timer, rt_hwtimerval_t *tv)
{
    rt_uint32_t counter, reload;
    rt_uint64_t timer_cnt, n1;
    int i, index = 0, n0;
    float tv_sec;
    rt_uint32_t dev, dev_min;

    /* changed to second */
    tv_sec = tv->sec + tv->usec/(float)1000000.0;
    timer_cnt = tv_sec * timer->freq;
    if (timer_cnt == 0) {
        timer_cnt = 1;
    }
    if (timer_cnt < timer->maxcnt) {
        timer->cycles = timer->reload = 1;
        timer->period_sec = tv_sec;
        counter = timer_cnt;
        return counter;
    }
    if (timer_cnt % timer->maxcnt == 0) {
        timer->cycles = timer->reload = timer_cnt / timer->maxcnt;
        timer->period_sec = tv_sec;
        counter = timer_cnt;
        return counter;
    }
    n0 = timer_cnt / timer->maxcnt + 1;
    n1 = timer_cnt / 2;
    dev_min = n0;
    for (i = n0; i < n1; i++) {
        reload = (rt_uint32_t)(timer_cnt / i);
        dev = timer_cnt - reload * i;
        if (dev <= 500) {
            // end
            index = i;
            break;
#ifdef HIGH_SPEED
        } else if (dev > dev_min) {
            break;
#endif
        } else if (dev < dev_min) {
            dev_min = dev;
            index = i;
        }
    }
    timer->cycles = timer->reload = index;
    timer->period_sec = index / timer->freq;
    counter = timer_cnt / index;
    return counter;
}

void presisionTest()
{
    rt_uint32_t exp, res, dev;
    int i;

    rt_hwtimer_t timer;
    rt_hwtimerval_t tv;
    timer.maxcnt = 0xFFFF;
    timer.freq = 1000000;

    _DEBUG("\n\n********************\nradom test");

    _DEBUG("1s");
    tv.sec = 1;
    tv.usec = 0;
    exp = (tv.sec + tv.usec/(float)1000000.0) * timer.freq;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload32(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n2s");
    tv.sec = 2;
    tv.usec = 0;
    exp = (tv.sec + tv.usec/(float)1000000.0) * timer.freq;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload32(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n3.317s");
    tv.sec = 3;
    tv.usec = 317000;
    exp = (tv.sec + tv.usec/(float)1000000.0) * timer.freq;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload32(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n5s");
    tv.sec = 5;
    tv.usec = 0;
    exp = (tv.sec + tv.usec/(float)1000000.0) * timer.freq;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload32(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n7.537s");
    tv.sec = 7;
    tv.usec = 537000;
    exp = (tv.sec + tv.usec/(float)1000000.0) * timer.freq;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload32(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n999.999s");
    tv.sec = 999;
    tv.usec = 999000;
    exp = (tv.sec*1000 + tv.usec/1000.0) * (timer.freq/1000.0);

    timer.counter = calcTimerReload1(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload32(&timer, &tv);
    res = timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

    _DEBUG("radom test end\n********************\n");

    _DEBUG("\n********************\ndev test");
    for (i = 1; i < 5; i++) {
        tv.sec = 0;
        tv.usec = 31000*i + 310*i;
        _DEBUG("--------------------\n%d.%06ds", tv.sec, tv.usec);
        exp = (tv.sec*1000 + tv.usec/1000.0) * (timer.freq/1000.0);

        timer.counter = calcTimerReload1(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

        timer.counter = calcTimerReload32(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);
    }
    for (i = 1; i < 15; i++) {
        tv.sec = i;
        tv.usec = 230970;
        _DEBUG("--------------------\n%d.%ds", tv.sec, tv.usec);
        exp = (tv.sec*1000 + tv.usec/1000.0) * (timer.freq/1000.0);

        timer.counter = calcTimerReload1(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

        timer.counter = calcTimerReload32(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);
    }
    for (i = 1000; i < 1005; i++) {
        tv.sec = i;
        tv.usec = 0;
        _DEBUG("--------------------\n%d.%ds", tv.sec, tv.usec);
        exp = (tv.sec*1000 + tv.usec/1000.0) * (timer.freq/1000.0);

        timer.counter = calcTimerReload1(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

        timer.counter = calcTimerReload32(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);
    }
    for (i = 3160; i < 3165; i++) {
        tv.sec = i;
        tv.usec = 0;
        _DEBUG("--------------------\n%d.%ds", tv.sec, tv.usec);
        exp = (tv.sec*1000 + tv.usec/1000.0) * (timer.freq/1000.0);

        timer.counter = calcTimerReload1(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

        timer.counter = calcTimerReload32(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);
    }
    for (i = 4290; i < 4294; i++) {
        tv.sec = i;
        tv.usec = 0;
        _DEBUG("--------------------\n%d.%ds", tv.sec, tv.usec);
        exp = (tv.sec*1000 + tv.usec/1000.0) * (timer.freq/1000.0);

        timer.counter = calcTimerReload1(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);

        timer.counter = calcTimerReload32(&timer, &tv);
        res = timer.counter * timer.cycles;
        dev = res - exp;
        _DEBUG("%d; %d; %d", timer.counter, timer.cycles, dev);
    }

    _DEBUG("dev test end\n********************\n");
}

void speedTest()
{
    int i;
    float tim1, tim2;
    LAU::LTimer ltim;

    rt_hwtimer_t timer;
    rt_hwtimerval_t tv;
    timer.maxcnt = 0xFFFF;
    timer.freq = 1000000;

    _DEBUG("\n********************\n1M times test");
    _DEBUG("--------------------\n3.317s");
    tv.sec = 3;
    tv.usec = 317000;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload32(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    tv.sec = 3;
    tv.usec = 317;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload32(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    _DEBUG("--------------------\n7.537s");
    tv.sec = 7;
    tv.usec = 537000;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload32(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    tv.sec = 7;
    tv.usec = 537;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload32(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    _DEBUG("--------------------\n999.999s");
    tv.sec = 999;
    tv.usec = 999000;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload32(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    tv.sec = 999;
    tv.usec = 999;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e6; i++) {
        timer.counter = calcTimerReload32(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    _DEBUG("1M times test end\n********************\n");
}

void longTest()
{
    rt_uint64_t exp, res, dev;
    int i;
    float tim1, tim2;
    LAU::LTimer ltim;

    rt_hwtimer_t timer;
    rt_hwtimerval_t tv;
    timer.maxcnt = 0xFFFF;
    timer.freq = 1000000;


    _DEBUG("\n\n********************\nlong timer test");


    _DEBUG("--------------------\n9999.537s");
    tv.sec = 9999;
    tv.usec = 537000;
    exp = tv.usec;
    exp = 1000000*(rt_uint64_t)tv.sec + exp;
//    exp *= timer.freq/1000000;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload64(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n19999.999s");
    tv.sec = 19999;
    tv.usec = 999000;
    exp = tv.usec;
    exp = 1000000*(rt_uint64_t)tv.sec + exp;
//    exp *= timer.freq/1000000;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload64(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n1999999.999s");
    tv.sec = 1999999;
    tv.usec = 999000;
    exp = tv.usec;
    exp = 1000000*(rt_uint64_t)tv.sec + exp;
//    exp *= timer.freq/1000000;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload64(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    _DEBUG("--------------------\n19999999.999s");
    tv.sec = 19999999;
    tv.usec = 999000;
    exp = tv.usec;
    exp = 1000000*(rt_uint64_t)tv.sec + exp;
//    exp *= timer.freq/1000000;

    timer.counter = calcTimerReload1(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    timer.counter = calcTimerReload64(&timer, &tv);
    res = (rt_uint64_t)timer.counter * timer.cycles;
    dev = res - exp;
    _DEBUG("%d; %d; %lld", timer.counter, timer.cycles, dev);

    _DEBUG("long timer test end\n********************\n");

    _DEBUG("long timer test end\n********************\n");


    _DEBUG("\n********************\n1M times test");
    _DEBUG("--------------------\n9999.537s");
    tv.sec = 9999;
    tv.usec = 537000;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e4; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e4; i++) {
        timer.counter = calcTimerReload64(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    _DEBUG("--------------------\n19999.999s");
    tv.sec = 19999;
    tv.usec = 999000;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e4; i++) {
        timer.counter = calcTimerReload1(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms", tim2 - tim1);

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e4; i++) {
        timer.counter = calcTimerReload64(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    _DEBUG("--------------------\n1999999.999s");
    tv.sec = 1999999;
    tv.usec = 999000;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e4; i++) {
        timer.counter = calcTimerReload64(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    _DEBUG("--------------------\n19999999.999s");
    tv.sec = 19999999;
    tv.usec = 999000;

    tim1 = ltim.elapsedMsec();
    for (i = 0; i < 1e4; i++) {
        timer.counter = calcTimerReload64(&timer, &tv);
    }
    tim2 = ltim.elapsedMsec();
    _DEBUG("1M times use: %.3fms\n", tim2 - tim1);

    _DEBUG("1M times test end\n********************\n");
}
